#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import socket
from std_msgs.msg import String

class UdpSenderNode:
    # 1. 在这里写死目标 IP 和端口
    UDP_IP = '194.194.194.101'
    UDP_PORT = 5005

    def __init__(self):
        rospy.init_node('udp_sender_node', anonymous=True)

        # 2. 创建 UDP 套接字
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        # 如果需要广播，可以启用下面一行
        # self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)

        # 3. 订阅 ROS 话题，将消息通过 UDP 转发
        self.sub = rospy.Subscriber('~in_topic', String, self.topic_callback)

        # 4. 定时发送心跳（可选）
        self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_callback)

        rospy.on_shutdown(self.shutdown_hook)
        rospy.loginfo("UDP Sender Node 启动，目标：%s:%d", self.UDP_IP, self.UDP_PORT)

    def topic_callback(self, msg: String):
        data = msg.data.encode('utf-8')
        try:
            self.sock.sendto(data, (self.UDP_IP, self.UDP_PORT))
            rospy.logdebug("通过 UDP 发送: %s", msg.data)
        except socket.error as e:
            rospy.logwarn("UDP 发送失败: %s", e)

    def timer_callback(self, event):
        heartbeat = "hello from ROS UDP\n".encode('utf-8')
        try:
            self.sock.sendto(heartbeat, (self.UDP_IP, self.UDP_PORT))
            rospy.logdebug("发送心跳")
        except socket.error as e:
            rospy.logwarn("UDP 心跳发送失败: %s", e)

    def shutdown_hook(self):
        rospy.loginfo("关闭 UDP 套接字")
        self.sock.close()


if __name__ == '__main__':
    try:
        node = UdpSenderNode()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
